#include "ros/ros.h"
#include "ros/console.h"

//Input: Vantage Points
#include "turtlebot_frontier/VantagePointArray.h"
#include "turtlebot_frontier/VantagePoint.h"

//Input: Occupancy Grid (really just for the resolution)
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"

//Output: Goal
#include "geometry_msgs/PoseStamped.h"

//Intermediary:
#include "geometry_msgs/PoseArray.h"
#include "turtlebot_slam_navigation/get_distances.h"
#include "turtlebot_slam_navigation/PoseWithDistance.h"

//Misc
#include <vector>
#include <math.h>

class TurtleExplorer {

public:

    //Initialize the publishers, subscribers and clients
    TurtleExplorer(ros::NodeHandle nh) {
        resolution = 0;
        mapClient = nh.serviceClient<nav_msgs::GetMap>("dynamic_map");
        mapSubscriber = nh.subscribe("map", 1, &TurtleExplorer::mapCallback, this);
        vantagePointSubscriber = nh.subscribe("vantage_points", 1, &TurtleExplorer::vantagePointsCallback, this);
        distanceClient = nh.serviceClient<turtlebot_slam_navigation::get_distances>("get_distances");
        goalPublisher = nh.advertise<geometry_msgs::PoseStamped>("goal", 1, true);
        poseArrayPublisher = nh.advertise<geometry_msgs::PoseArray>("vantage_posearray", 1, true);
        ros::spinOnce();
    }

    //Spin calls the map client twice per second until it first gets a response,
    //then every 10 seconds
    void spin() {

        ros::Rate rate(0.1);
		int cycle = 0;
        while(ros::ok()) {
			if(resolution == 0 || (cycle % 20) == 0) {
				nav_msgs::GetMap map_req;
				if(mapClient.call(map_req)) {
					ROS_INFO("Map got");
					resolution = map_req.response.map.info.resolution;
					mapX = map_req.response.map.info.origin.position.x;
					mapY = map_req.response.map.info.origin.position.y;
				}
			}
            ros::spinOnce();
            rate.sleep();
            cycle++;
        }
    }

    //Callbacks

    void mapCallback(nav_msgs::OccupancyGrid msg) {
        //Store the resolution and the origin
        resolution = msg.info.resolution;
        mapX = msg.info.origin.position.x;
        mapY = msg.info.origin.position.y;
        ROS_INFO("Map got");
    }

    void vantagePointsCallback(turtlebot_frontier::VantagePointArray msg) {
        ROS_INFO("Vantage points callback called");
        //Only act if we have a valid resolution stored somewhere and we actually have vantage points
        if(resolution > 0 && msg.vantagePoints.size() > 0) {
            //Step 1: make a PoseArray
            geometry_msgs::PoseArray poses;
            poses.header.frame_id = "map";
            poses.poses.resize(msg.vantagePoints.size());

            //Step 2: fill it up
            for(int i=0; i<msg.vantagePoints.size(); i++) {
				//Convert the map grid into meters
                poses.poses[i].position.x = (double)msg.vantagePoints[i].x * resolution + mapX;
                poses.poses[i].position.y = (double)msg.vantagePoints[i].y * resolution + mapY;
                poses.poses[i].position.z = 0;
                poses.poses[i].orientation.x = 0;
                poses.poses[i].orientation.y = 0;
                poses.poses[i].orientation.z = 1;
				//A different angle so the arrows would be easier to discern in rviz
                poses.poses[i].orientation.w = M_PI/2;
            }
            ROS_INFO("PoseArray filled up.");

			//Publish the vantage points for visualization
            poseArrayPublisher.publish(poses);

            //Step 3: get the distances
            turtlebot_slam_navigation::get_distances request;
            request.request.poses = poses;
            distanceClient.call(request);
            ROS_INFO("Distances got");

            //Step 4: select the best one
            int bestOne = 0;
            if(request.response.distances.size() == 0) {
                ROS_ERROR("Error: response.distances.size() == 0!");
                return;
            }
            double bestValue = msg.vantagePoints[0].value / sqrt(request.response.distances[0].distance);
            bool foundBest = false;

			//First, see if there is a vantage point outside the minimal range
            for(int i=1; i<msg.vantagePoints.size(); i++) {
                double cValue = msg.vantagePoints[i].value / sqrt(request.response.distances[i].distance);
                if(cValue > bestValue && request.response.distances[i].distance >= MIN_DISTANCE) {
                    bestValue = cValue;
                    bestOne = i;
                    foundBest = true;
                }
            }

			//If there wasn't, check inside the minimal range too
            if(!foundBest) {
                for(int i=1; i<msg.vantagePoints.size(); i++) {
                    double cValue = msg.vantagePoints[i].value / sqrt(request.response.distances[i].distance);
                    if(cValue > bestValue) {
                        bestValue = cValue;
                        bestOne = i;
                        foundBest = true;
                    }
                }

            }

            ROS_INFO("Best point selected (distance: %f, value: %d)", request.response.distances[bestOne].distance, msg.vantagePoints[bestOne].value);

            //Step 5: publish the best one as "goal"

            geometry_msgs::PoseStamped output;
            output.header.frame_id = "map";
            output.header.stamp = ros::Time::now();
            output.pose = poses.poses[bestOne];

            goalPublisher.publish(output);

            ROS_INFO("Goal published");

        }
        else if(msg.vantagePoints.size() == 0) {
            ROS_WARN("No vantage points");
        }
        else {
            ROS_WARN("Map wasn't received, no stored resolution");
        }
    }

    //Node stuff
    ros::Subscriber vantagePointSubscriber;
    ros::Subscriber mapSubscriber;
    ros::ServiceClient mapClient;

    ros::ServiceClient distanceClient;

    ros::Publisher goalPublisher;

    //Rviz
    ros::Publisher poseArrayPublisher;

    //Internal stuff
    double resolution;
    double mapX;
    double mapY;

    static const double MIN_DISTANCE = 0.75;
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "turtle_expl"); // Initiate new ROS node named "turtle_expl"
    ros::NodeHandle n;
    TurtleExplorer explorer(n); // Create a new TurtleExplorer object
    explorer.spin(); // Execute main loop
    return 0;
};
